import json
from typing import Dict, Any, Tuple, Optional, List
from .obstaclearray2markarray import process_lidar_perception_obstacle_array, process_lidar_perception_obstacle_array_info
from .baselink2markandposestamp import process_baselink_tf_to_pose, process_baselink_tf_to_marker, process_baselink_tf_to_pointstamped, process_baselink_tf_to_vehicle_marker
from .map2markarray import process_map_to_marker_array
from .scan_init_down_reverse import process_scan_down_right_reverse, process_scan_down_left_reverse, process_scan_init_down_reverse

try:
    from gglobal import logger
except ImportError:
    import logging
    logger = logging.getLogger(__name__)

async def process_custom_conversions_for_bag(topic_name: str, msg_dict: Dict[str, Any], 
                                           timestamp: int, writer, generated_connections: Dict[str, Any],
                                           typestore, dict_to_msg_func):
    """处理自定义话题转换（用于bag文件转换）"""
    try:
        # 处理/lidar_perception/obstacle_array话题
        if topic_name == "/lidar_perception/obstacle_array":
            await handle_obstacle_array_conversion_for_bag(
                msg_dict, timestamp, writer, generated_connections, typestore, dict_to_msg_func
            )
        
        # 处理/baselink_tf话题
        elif topic_name == "/baselink_tf":
            await handle_baselink_tf_conversion_for_bag(
                msg_dict, timestamp, writer, generated_connections, typestore, dict_to_msg_func
            )
        
        # 处理/map话题
        elif topic_name == "/map":
            await handle_map_conversion_for_bag(
                msg_dict, timestamp, writer, generated_connections, typestore, dict_to_msg_func
            )
        
        # 处理下激光话题反转
        elif topic_name in ["/scan_down_right", "/scan_down_left", "/scan_init_down"]:
            await handle_scan_reverse_conversion_for_bag(
                topic_name, msg_dict, timestamp, writer, generated_connections, typestore, dict_to_msg_func
            )
            
    except Exception as e:
        logger.error(f"处理自定义转换时出错: {e}", exc_info=True)


async def handle_obstacle_array_conversion_for_bag(msg_dict: Dict[str, Any], timestamp: int, 
                                                  writer, generated_connections: Dict[str, Any],
                                                  typestore, dict_to_msg_func):
    """处理obstacle_array话题转换（用于bag文件转换）"""
    try:
        # 生成convex_hull_box数据
        marker_array_data = process_lidar_perception_obstacle_array(msg_dict)
        await write_converted_message_to_bag(
            "/lidar_perception/obstacle_array/convex_hull_box",
            "visualization_msgs/msg/MarkerArray",
            marker_array_data,
            timestamp,
            writer,
            generated_connections,
            typestore,
            dict_to_msg_func
        )
        
        # 生成info数据
        info_marker_array_data = process_lidar_perception_obstacle_array_info(msg_dict)
        await write_converted_message_to_bag(
            "/lidar_perception/obstacle_array/info",
            "visualization_msgs/msg/MarkerArray",
            info_marker_array_data,
            timestamp,
            writer,
            generated_connections,
            typestore,
            dict_to_msg_func
        )
        
    except Exception as e:
        logger.error(f"处理obstacle_array转换时出错: {e}", exc_info=True)


async def handle_baselink_tf_conversion_for_bag(msg_dict: Dict[str, Any], timestamp: int,
                                               writer, generated_connections: Dict[str, Any],
                                               typestore, dict_to_msg_func):
    """处理baselink_tf话题转换（用于bag文件转换）"""
    try:
        # 生成PoseStamped数据
        pose_stamped_data = process_baselink_tf_to_pose(msg_dict)
        await write_converted_message_to_bag(
            "/baselink_tf/topose",
            "geometry_msgs/msg/PoseStamped",
            pose_stamped_data,
            timestamp,
            writer,
            generated_connections,
            typestore,
            dict_to_msg_func
        )
        
        # 生成Marker数据
        marker_data = process_baselink_tf_to_marker(msg_dict)
        await write_converted_message_to_bag(
            "/baselink_tf/mark",
            "visualization_msgs/msg/Marker",
            marker_data,
            timestamp,
            writer,
            generated_connections,
            typestore,
            dict_to_msg_func
        )
        
        # 生成PointStamped数据
        pointstamped_data = process_baselink_tf_to_pointstamped(msg_dict)
        await write_converted_message_to_bag(
            "/baselink_tf/point",
            "geometry_msgs/msg/PointStamped",
            pointstamped_data,
            timestamp,
            writer,
            generated_connections,
            typestore,
            dict_to_msg_func
        )
        
        # 生成车辆模型Marker数据
        vehicle_marker_data = process_baselink_tf_to_vehicle_marker(msg_dict)
        if vehicle_marker_data is not None:
            await write_converted_message_to_bag(
                "/vehicle/mark",
                "visualization_msgs/msg/Marker",
                vehicle_marker_data,
                timestamp,
                writer,
                generated_connections,
                typestore,
                dict_to_msg_func
            )
        
    except Exception as e:
        logger.error(f"处理baselink_tf转换时出错: {e}", exc_info=True)


async def handle_map_conversion_for_bag(msg_dict: Dict[str, Any], timestamp: int,
                                        writer, generated_connections: Dict[str, Any],
                                        typestore, dict_to_msg_func):
    """处理/map话题转换（用于bag文件转换）"""
    try:
        # 获取localmap参数
        from gglobal import config
        localmap_path = config.get_param('localmap', 'None')
        
        # 检查localmap路径
        if localmap_path == 'None' or not localmap_path:
            logger.info("localmap参数为空，跳过/map话题转换")
            return
        
        # 清理路径中的引号
        localmap_path = localmap_path.strip('"').strip("'")
        
        import os
        if not os.path.exists(localmap_path):
            logger.warning(f"localmap文件不存在: {localmap_path}，跳过/map话题转换")
            return
        
        logger.info(f"开始处理/map话题转换，使用地图文件: {localmap_path}")
        
        # 生成MarkerArray数据
        marker_array_data = process_map_to_marker_array(msg_dict, localmap_path)
        
        # 写入转换后的消息
        await write_converted_message_to_bag(
            "/map_line",
            "visualization_msgs/msg/MarkerArray",
            marker_array_data,
            timestamp,
            writer,
            generated_connections,
            typestore,
            dict_to_msg_func
        )
        
    except Exception as e:
        logger.error(f"处理/map转换时出错: {e}", exc_info=True)


async def handle_scan_reverse_conversion_for_bag(topic_name: str, msg_dict: Dict[str, Any], timestamp: int,
                                                writer, generated_connections: Dict[str, Any],
                                                typestore, dict_to_msg_func):
    """处理激光扫描数据反转转换（用于bag文件转换）"""
    try:
        # 根据话题名称选择对应的处理函数
        if topic_name == "/scan_down_right":
            reversed_data = process_scan_down_right_reverse(msg_dict)
            new_topic_name = "/scan_down_right_reverse"
        elif topic_name == "/scan_down_left":
            reversed_data = process_scan_down_left_reverse(msg_dict)
            new_topic_name = "/scan_down_left_reverse"
        elif topic_name == "/scan_init_down":
            reversed_data = process_scan_init_down_reverse(msg_dict)
            new_topic_name = "/scan_init_down_reverse"
        else:
            logger.warning(f"未知的激光话题: {topic_name}")
            return
        
        # 写入转换后的消息
        await write_converted_message_to_bag(
            new_topic_name,
            "sensor_msgs/msg/LaserScan",
            reversed_data,
            timestamp,
            writer,
            generated_connections,
            typestore,
            dict_to_msg_func
        )
        
        logger.debug(f"成功处理激光数据反转: {topic_name} -> {new_topic_name}")
        
    except Exception as e:
        logger.error(f"处理激光数据反转时出错 {topic_name}: {e}", exc_info=True)


async def write_converted_message_to_bag(topic_name: str, msg_type: str, msg_data: Dict[str, Any],
                                        timestamp: int, writer, generated_connections: Dict[str, Any],
                                        typestore, dict_to_msg_func):
    """将转换后的消息写入bag文件"""
    try:
        # 检查是否已经创建了该话题的连接
        if topic_name not in generated_connections:
            # 创建新的连接
            connection = writer.add_connection(
                topic=topic_name,
                msgtype=msg_type
            )
            generated_connections[topic_name] = connection
            logger.debug(f"创建新连接: {topic_name} ({msg_type})")
        
        connection = generated_connections[topic_name]
        
        # 获取消息类型定义并创建消息对象
        msgdef = typestore.get_msgdef(msg_type)
        msg_obj = dict_to_msg_func(msg_data, msgdef)
        
        # 序列化消息对象
        serialized_data = typestore.serialize_ros1(msg_obj, msg_type)
        
        # 写入bag文件
        writer.write(connection, timestamp, serialized_data)
        logger.debug(f"写入转换后的消息: {topic_name}")
        
    except Exception as e:
        logger.error(f"写入转换后的消息失败 {topic_name}: {e}", exc_info=True)